using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using UnityEngine.Assertions;

public class MapPath
{
    private static List<TriangleData> _listcheck = new List<TriangleData>();
    private static List<TriangleData> _listfinish = new List<TriangleData>();

    public static void CountPath()
    {
        var startPos = MapDrawer.GetInstance().StartPos;
        var endPos = MapDrawer.GetInstance().EndPos;

        var startIdx = GetMapIdx(startPos);
        var endIdx = GetMapIdx(endPos);

        Debug.Log("startIdx:" + startIdx + ",endIdx:" + endIdx);

        Debug.Assert(startIdx >= 0, "startPos寻路点不在地图内:" + startIdx);
        Debug.Assert(endIdx >= 0, "endPos寻路点不在地图内:" + endIdx);

        if (startIdx < 0 || endIdx < 0)
        {
            return;
        }

        Debug.Log("开始寻路");
        MapDrawer.GetInstance().ClearPath();

        var maps = MapDrawer.GetInstance().Map;
        //1.清空寻路缓存，设置相邻关系点图
        float dis = 0.05f;
        for (int i = 0; i < maps.Count; ++i)
        {
            maps[i].Reset();
        }

        for (int i = 0; i < maps.Count; ++i)
        {
            var data1 = maps[i];
            for (int j = 0; j < maps.Count; ++j)
            {
                var data2 = maps[j];
                if (i == j)
                {
                    continue;
                }

                int bingleCount = 0;
                bool hitA = false;
                bool hitB = false;
                bool hitC = false;
                if ((data1.A - data2.A).magnitude < dis ||
                    (data1.A - data2.B).magnitude < dis ||
                    (data1.A - data2.C).magnitude < dis)
                {
                    ++bingleCount;
                    hitA = true;
                }

                if ((data1.B - data2.A).magnitude < dis ||
                    (data1.B - data2.B).magnitude < dis ||
                    (data1.B - data2.C).magnitude < dis)
                {
                    ++bingleCount;
                    hitB = true;
                }

                if ((data1.C - data2.A).magnitude < dis ||
                    (data1.C - data2.B).magnitude < dis ||
                    (data1.C - data2.C).magnitude < dis)
                {
                    ++bingleCount;
                    hitC = true;
                }

                if (bingleCount > 1)
                {
                    TryToAddNeighbour(data1, data2, hitA, hitB, hitC);
                }
            }
        }

        //2.计算预估代价
        _listcheck.Clear();
        _listfinish.Clear();
        TriangleData nowTriangle;
        TryToAddToListcheck(maps[startIdx]);
        int ci = 0;
        while ((nowTriangle = GetPbByMinF()) != null)
        {
            ++ci;
            //Debug.Log(ci);
            TriangleData pbend = maps[endIdx];
            if (nowTriangle.NeighborA == pbend ||
                nowTriangle.NeighborB == pbend ||
                nowTriangle.NeighborC == pbend)
            {
                _listfinish.Add(pbend);
                pbend.Father = nowTriangle;
                while (pbend.Father != null)
                {
                    //这是网格路径
                    MapDrawer.GetInstance().AddGridPath(new Line
                    {
                        From = pbend.GetCenterPosition(),
                        To = pbend.Father.GetCenterPosition()
                    });
                    pbend.Father.Next = pbend;
                    pbend = pbend.Father;
                }

                //网格结合
                OptimizationPath(pbend, startPos, endPos);
                break;
            }

            if (CountNeighbourF(nowTriangle, nowTriangle.NeighborA))
            {
                nowTriangle.NeighborA.H =
                    (nowTriangle.NeighborA.GetCenterPosition() - pbend.GetCenterPosition()).magnitude;
                nowTriangle.NeighborA.F = nowTriangle.NeighborA.H + nowTriangle.NeighborA.G;
            }

            if (CountNeighbourF(nowTriangle, nowTriangle.NeighborB))
            {
                nowTriangle.NeighborB.H =
                    (nowTriangle.NeighborB.GetCenterPosition() - pbend.GetCenterPosition()).magnitude;
                nowTriangle.NeighborB.F = nowTriangle.NeighborB.H + nowTriangle.NeighborB.G;
            }

            if (CountNeighbourF(nowTriangle, nowTriangle.NeighborC))
            {
                nowTriangle.NeighborC.H =
                    (nowTriangle.NeighborC.GetCenterPosition() - pbend.GetCenterPosition()).magnitude;
                nowTriangle.NeighborC.F = nowTriangle.NeighborC.H + nowTriangle.NeighborC.G;
            }

            AddToFinish(nowTriangle);
        }

        Debug.Log("寻路完成");
    }

    //中心点到下一个临近三角形的顺时针顶点与向量（Unity的NavMesh数据三角形为顺时针排列）
    private static void CaculateV(Vector3 startPosition, TriangleData tCount, ref Vector3 v1, ref Vector3 v2,
        ref Vector3 p1,
        ref Vector3 p2)
    {
        if (tCount == null)
        {
            return;
        }

        if (tCount.Next != null)
        {
            if (tCount.Next == tCount.NeighborA)
            {
                v1 = tCount.B - startPosition;
                v2 = tCount.C - startPosition;
                p1 = tCount.B;
                p2 = tCount.C;
            }
            else if (tCount.Next == tCount.NeighborB)
            {
                v1 = tCount.A - startPosition;
                v2 = tCount.C - startPosition;
                p1 = tCount.A;
                p2 = tCount.C;
            }
            else if (tCount.Next == tCount.NeighborC)
            {
                v1 = tCount.B - startPosition;
                v2 = tCount.A - startPosition;
                p1 = tCount.B;
                p2 = tCount.A;
            }

            //start Position和另一个V，在同一个边上则边就确定了，从这个三角形中心找左右
            if (Vector3.Cross(new Vector3(v1.x, 0, v1.z), new Vector3(v2.x, 0, v2.z)).y == 0)
            {
                Vector3 vcp_1 = p1 - tCount.GetCenterPosition();
                Vector3 vcp_2 = p2 - tCount.GetCenterPosition();

                //边反了
                if (Vector3.Cross(new Vector3(vcp_1.x, 0, vcp_1.z), new Vector3(vcp_2.x, 0, vcp_2.z)).y < 0)
                {
                    v1 = p2 - startPosition;
                    v2 = p1 - startPosition;
                    Vector3 pswitch = p1;
                    p1 = p2;
                    p2 = pswitch;
                }
                else
                {
                    v1 = p1 - startPosition;
                    v2 = p2 - startPosition;
                }
            }
            //左手坐标系，所以小于等于
            else if (Vector3.Cross(new Vector3(v1.x, 0, v1.z), new Vector3(v2.x, 0, v2.z)).y < 0)
            {
                Vector3 vswitch = v1;
                Vector3 pswitch = p1;
                v1 = v2;
                v2 = vswitch;
                p1 = p2;
                p2 = pswitch;
            }
        }
        else
        {
            v1 = v2 = tCount.GetCenterPosition() - startPosition;
            p1 = p2 = tCount.GetCenterPosition();
        }
    }

    private static void CountNext(ref List<Vector3> listpath, ref TriangleData t)
    {
        if (t.Next == null)
        {
            listpath.Add(t.GetCenterPosition());
        }

        t = t.Next;
    }

    private static void OptimizationPath(TriangleData start, Vector3 startPos, Vector3 endPos)
    {
        Vector3 v1 = new Vector3();
        Vector3 v2 = new Vector3();
        Vector3 v3 = new Vector3();
        Vector3 v4 = new Vector3();

        Vector3 p1 = new Vector3();
        Vector3 p2 = new Vector3();
        Vector3 p3 = new Vector3();
        Vector3 p4 = new Vector3();
        TriangleData g1 = start;
        TriangleData g2 = start;
        List<Vector3> listpath = new List<Vector3>();
        Vector3 startPosition = start.GetCenterPosition();
        listpath.Add(startPosition);
        TriangleData tCount = start.Next;
        CaculateV(startPosition, start, ref v1, ref v2, ref p1, ref p2);
        while (tCount != null)
        {
            CaculateV(startPosition, tCount, ref v3, ref v4, ref p3, ref p4);

            //当前视野
            MapDrawer.GetInstance().AddRay("v1", new Ray
            {
                From = startPosition + Vector3.up,
                Dir = v1,
                Col = new Color(186 / 255f, 62 / 255f, 224 / 255f)
            });
            MapDrawer.GetInstance().AddRay("v2", new Ray
            {
                From = startPosition + Vector3.up,
                Dir = v2,
                Col = new Color(21 / 255f, 107 / 255f, 141 / 255f)
            });
            //下一视野
            MapDrawer.GetInstance().AddRay("v3", new Ray
            {
                From = startPosition,
                Dir = v3,
                Col = new Color(246 / 255f, 133 / 255f, 59 / 255f)
            });
            MapDrawer.GetInstance().AddRay("v4", new Ray
            {
                From = startPosition,
                Dir = v4,
                Col = new Color(228 / 255f, 6 / 255f, 27 / 255f)
            });

            //这里的算法是v1,v2范围包含v3,v4,如果不包含，说明出现拐点

            //左手坐标系，所以小于等于
            if (Vector3.Cross(new Vector3(v3.x, 0, v3.z), new Vector3(v2.x, 0, v2.z)).y <= 0)
            {
                // startPosition = p2;
                // listpath.Add(g2.GetCenterPosition());
                startPosition = g2.GetCenterPosition();
                tCount = g2;
                g2 = tCount.Next;
                CaculateV(startPosition, tCount, ref v1, ref v2, ref p1, ref p2);
                listpath.Add(startPosition);
                CountNext(ref listpath, ref tCount);
                MapDrawer.GetInstance().AddRay("cross", new Ray
                {
                    From = startPosition,
                    Dir = Vector3.up * 100,
                    Col = Color.yellow
                });
                continue;
            }

            //左手坐标系，所以大于等于
            if (Vector3.Cross(new Vector3(v4.x, 0, v4.z), new Vector3(v1.x, 0, v1.z)).y >= 0)
            {
                // startPosition = p1;
                // listpath.Add(g1.GetCenterPosition());
                startPosition = g1.GetCenterPosition();
                tCount = g1;
                g1 = tCount.Next;
                CaculateV(startPosition, tCount, ref v1, ref v2, ref p1, ref p2);
                listpath.Add(startPosition);
                CountNext(ref listpath, ref tCount);
                MapDrawer.GetInstance().AddRay("cross", new Ray
                {
                    From = startPosition,
                    Dir = Vector3.up * 100,
                    Col = Color.black
                });
                continue;
            }

            MapDrawer.GetInstance().AddRay("cross", null);

            //左手坐标系，所以小于等于（但不能反向）
            var v3_1_normal = Vector3.Cross(new Vector3(v3.x, 0, v3.z), new Vector3(v1.x, 0, v1.z));
            if (v3_1_normal.y < 0 || (v3_1_normal.y == 0 && Vector3.Normalize(v3).Equals(Vector3.Normalize(v1))))
            {
                v1 = v3;
                p1 = p3;
                g1 = tCount.Next;
            }

            //左手坐标系，所以小于等于
            var v4_2_normal = Vector3.Cross(new Vector3(v4.x, 0, v4.z), new Vector3(v2.x, 0, v2.z));
            if (v4_2_normal.y > 0 || (v4_2_normal.y == 0 && Vector3.Normalize(v4).Equals(Vector3.Normalize(v2))))
            {
                v2 = v4;
                p2 = p4;
                g2 = tCount.Next;
            }

            CountNext(ref listpath, ref tCount);
        }

        //头尾改为目标点
        listpath[0] = startPos;
        listpath[listpath.Count - 1] = endPos;

        for (int i = 0; i < listpath.Count - 1; ++i)
        {
            MapDrawer.GetInstance().AddPath(new Line
            {
                From = listpath[i],
                To = listpath[i + 1]
            });
        }
    }

    private static bool CountNeighbourF(TriangleData a, TriangleData neighbour)
    {
        if (neighbour == null || neighbour.Finished)
        {
            return false;
        }

        float G = CountDis(a, neighbour);
        if (neighbour.G == 0f || (a.G + G) < neighbour.G)
        {
            neighbour.G = a.G + G;
            neighbour.Father = a;
            TryToAddToListcheck(neighbour);
            return true;
        }

        return false;
    }

    private static float CountDis(TriangleData ta, TriangleData tb)
    {
        Vector3 ACenter = ta.GetCenterPosition();
        Vector3 BCenter = tb.GetCenterPosition();
        return (ACenter - BCenter).magnitude;
    }

    private static void AddToFinish(TriangleData t)
    {
        t.Finished = true;
        _listfinish.Add(t);
        _listcheck.Remove(t);
    }

    private static TriangleData GetPbByMinF()
    {
        TriangleData pb = null;
        for (int i = 0; i < _listcheck.Count; ++i)
        {
            if (pb == null || pb.F > _listcheck[i].F)
            {
                pb = _listcheck[i];
            }
        }

        return pb;
    }

    private static void TryToAddToListcheck(TriangleData t)
    {
        for (int i = 0; i < _listfinish.Count; ++i)
        {
            if (t.Id == _listfinish[i].Id)
            {
                return;
            }
        }

        for (int i = 0; i < _listcheck.Count; ++i)
        {
            if (t.Id == _listcheck[i].Id)
            {
                return;
            }
        }

        _listcheck.Add(t);
    }

    private static int GetMapIdx(Vector3 pos)
    {
        var maps = MapDrawer.GetInstance().Map;
        for (int i = 0; i < maps.Count; i++)
        {
            var t = maps[i];
            if (t.IsPointInTriangle(pos))
            {
                return i;
            }
        }

        return -1;
    }

    private static void TryToAddNeighbour(TriangleData baseTriangle, TriangleData neighbour, bool hitA, bool hitB,
        bool hitC)
    {
        if (hitA && hitB)
        {
            baseTriangle.NeighborC = neighbour;
        }
        else if (hitA && hitC)
        {
            baseTriangle.NeighborB = neighbour;
        }
        else if (hitC && hitB)
        {
            baseTriangle.NeighborA = neighbour;
        }
    }
}